#include <serial_comm.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctime>
#include <unistd.h>
#include <iostream>

int main(int argc, char* argv[]){
  SerialComm comm = SerialComm("/dev/ttyACM0", 115200);
  printf("%d\n", comm.isValid());

  printf("%ld\n", clock());
  //  printf("%ld\n", clock());
  char* input = new char[100];
  //  char* time = new char[100];
  int count = 0;
  //  time_t start = time(NULL);
  while(true/*count < atoi(argv[1])*/){
    count++;
    unsigned int nbytes = 100;
    printf("Enter string to send: ");
    getline(&input, &nbytes, stdin);
    //    printf("Time to wait: ");
    //    getline(&time, &nbytes, stdin);
    //    double wait = atof(time);
    //    printf("Waiting: %f\n", wait);

    int size = strlen(input);
    unsigned char* converted = new unsigned char[size];
    for(int i = 0; i < size; ++i){
      converted[i] = (unsigned char)(input[i]);
      }
    
    /*const char* stuff = "@z#z#z#z#z#z#z#z$";
    unsigned char* converted = new unsigned char[17];
    int size = 17;
    for(int i = 0 ;i < size; ++i){
      converted[i] = stuff[i];
    }
    
    printf("%d\n", count);
    printf("%s\n", converted);
    for(int i = 0; i < 17; ++i){
      printf("%c | ", converted[i]);
    }
    printf("\n");*/

    if(comm.sendString(converted, size) == FAILURE_COMM){
      fprintf(stderr, "Unable to send string.\n");
      return -1;
    }
    delete converted;

    //    usleep(wait);
    //    unsigned char* buff;
    //    while(comm.readString(&buff, '$') == FAILURE_COMM){
    //      fprintf(stderr, "Unable to read string.\n");
    //    }
    //    printf("Sucess! %s | \n", buff);
    usleep(16666);
  }
  std::cout << (double)clock()/CLOCKS_PER_SEC << std::endl;
}
